"""
根据位置式离散PID公式 
out=Kp*error(t)+Ki*Sum_error(t)+Kd*[error(t)-error(t-1)]
"""

previous_error = 0#当前误差
intergral = 0#积分
#measured_value 采样得到值
start:
    error = setpoint - measured_value #上次偏差
    intergral = intergral+error*dt #积分和
    derivative = (error -previous_error)/t  #微分
    output = Kp*error + ki*integral+kd*derivative # 输出
    previous_error = error #当前误差
    wait(dt)  #等待dt秒
    goto start

"""
根据增量式离散PID公式 
out=Kp*[error(t)-error(k-1)]+Ki*error(k)+Kd[error(k)-2error(k-1)+error(k-2)]
"""

error2 =0#上上次偏差
error1 = 0#上次偏差
out =0 #输出

loop:
    error0 = setpoint -messured_value #本次误差
    derivative = (error0-2*error1+error2)/dt #微分
    out = Kp * (error0-error1)+Ki*error0+Kd*derivative

    #记录偏差
    error2=error1
    error1=error0
    wiat(dt)
    goto loop
